
#ifndef SENSOR_MODEL_H
#define SENSOR_MODEL_H

#include "ros/ros.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseArray.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/Path.h"
#include "nav_msgs/MapMetaData.h"
#include "kdtree.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

#define SHOW_BEAM 0
/*
class SensorModel
{
public:
	SensorModel();
	~SensorModel();

	virtual bool update(geometry_msgs::PoseStamped pose, std::vector<int>& data) =0;

protected:
	ros::NodeHandle n;
	ros::ServiceClient map_client;
};
*/

class SensorModel
{
private:
	ros::NodeHandle& n;
	ros::NodeHandle& pn;

	double z_hit, z_short, z_max, z_rand, sigma_hit, lambda_short, max_occ_dist;
	double X_max, Y_max;// mins are just 0s
	double x_offset, y_offset;
	double raycastStep;
	int sweepStep, model_type;

	struct kdtree *ptree;
	nav_msgs::MapMetaData meta_data;

	ros::Publisher pcl_pub;
	ros::ServiceClient map_client;
	nav_msgs::OccupancyGrid Omap;

	std::vector<double> LF_map;
	std::vector<std::pair<double, double> > free_area;

	boost::mt19937& gen;
//	boost::variate_generator<boost::mt19937&, boost::uniform_int<> >  uniform_Int;
	boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uniform_2pi;

public:
SensorModel(ros::NodeHandle& nh, ros::NodeHandle& ph, boost::mt19937& g);
	~SensorModel();

	double getWeight(geometry_msgs::Pose& pose, std::vector<int>& data);

	void addNRandParticles(const int N, std::vector<geometry_msgs::Pose>& cloud);

	void publish_bestMeasurement(geometry_msgs::Pose&, std::vector<int>& data);

	geometry_msgs::Pose getOneRandParticle();

protected:

	double getWeightBeam(geometry_msgs::Pose& pose, std::vector<int>& data);

	double getWeightLF(geometry_msgs::Pose& pose, std::vector<int>& data);

	double raytrace(double x0, double y0, double x1, double y1);

	void initializeMap();

	void requestMap();

	double minDist(double x, double y);

	inline bool queryPtOnMap(double x, double y);

	inline int xy2Idx(double x, double y);

	inline double prob(double d, double sigma);

	inline double getXmapFrame(int grid_x);

	inline double getYmapFrame(int grid_y);
};

#endif
